/* http://www.apache.org/licenses/LICENSE-2.0 */
/* Many thanks to the writor of libaco Sen Han <00hnes@gmail.com> */

#define _GNU_SOURCE

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdint.h>
#include "mco.h"

#ifdef MCO_USE_VALGRIND
	#include <valgrind/valgrind.h>
#endif

#ifdef __x86_64__
#else
	#error "platform no support yet"
#endif

#define MCO_STACK_MAGIC	0xcabc5134fbfb3876

#define MCO_REG_IDX_RETADDR 4
#define MCO_REG_IDX_SP 5
#define MCO_REG_IDX_BP 7
#define MCO_REG_IDX_FPU 8

extern void * mco_ctx_switch(void * from_reg, void * to_reg) __asm__("mco_ctx_switch"); // asm
extern void mco_save_fpucw_mxcsr(void* p) __asm__("mco_save_fpucw_mxcsr");  // asm

typedef struct _mco_t	{
	void * reg[9];				/* CPU registers' state */
	void * switch_back_reg[9];	/* CPU registers' state */
	void * arg;
	char   is_end;
#ifdef MCO_USE_VALGRIND
	unsigned long valgrind_stk_id;
#endif
	char stk_frame[];
} mco_t;

/* mco's Global Thread Local Storage variable `co` */
__thread mco_t * mco_gtls_co;

static __thread void * mco_gtls_fpucw_mxcsr[1];

void
mco_thread_init(void)	{
	mco_save_fpucw_mxcsr(mco_gtls_fpucw_mxcsr);
}

int
mco_check_stack(void * mco_ptr)	{
	mco_t * p = mco_ptr;

	if (*((void**)(p->stk_frame)) == (void*)(MCO_STACK_MAGIC))	{
		return 0;
	}
	else	{
		return -1;
	}
}

static void
mco_exit(void)	{
	#if defined(__pic__) || defined(__PIC__)
		/* printf("PIC: called mco_exit() by return\n"); */
	#else
		/* printf("called mco_exit() by return\n"); */
	#endif

	mco_t * p;

	p = mco_gtls_co;

	p->is_end = 1;
	mco_ctx_switch((mco_gtls_co)->reg, (mco_gtls_co)->switch_back_reg);
}

void *
mco_create(mco_cofuncp_t fp, void * arg, size_t stk_sz_param)	{
	void * stk_align_retptr;
	size_t stk_msk;
	size_t stk_sz;
	uintptr_t u_p;
	mco_t * p;

	if(stk_sz_param <= 4096)	{
		stk_sz = 4096;
	}
	else	{
		stk_msk = 31;
		stk_sz = ((stk_sz_param + stk_msk) & ~stk_msk);
	}

	p = (mco_t *)malloc(sizeof(mco_t) + stk_sz);
	if (!p)	{
		return NULL;
	}

	memset(p, 0, sizeof(mco_t));

#ifdef MCO_USE_VALGRIND
	p->valgrind_stk_id = VALGRIND_STACK_REGISTER(p->stk_frame, (void*)((uintptr_t)p->stk_frame + stk_sz));
#endif
	u_p = (uintptr_t)(stk_sz - (sizeof(void*) << 1) + (uintptr_t)p->stk_frame);
	u_p = (u_p >> 4) << 4;
	stk_align_retptr  = (void*)(u_p - sizeof(void*));
	*((void**)(p->stk_frame)) = (void*)(MCO_STACK_MAGIC);
	*((void**)(stk_align_retptr)) = (void*)(mco_exit);

	p->reg[MCO_REG_IDX_RETADDR] = (void*)fp;
	p->reg[MCO_REG_IDX_SP] = stk_align_retptr;
	p->reg[MCO_REG_IDX_FPU] = mco_gtls_fpucw_mxcsr[0];	/* MCO_CONFIG_SHARE_FPU_MXCSR_ENV */
	p->arg = arg;

	return p;
}

/* __attribute__((__no_sanitize_address__)) */
void
mco_resume(void * resume_co_ptr)	{

	mco_t * resume_co = resume_co_ptr;

	if (!resume_co)	{
		return;
	}

	if (mco_gtls_co)	{
		printf("%s, %d: error: supposed to be called outside of a co\n", __FILE__, __LINE__);
		return;
	}

	if (resume_co->is_end)	{
		printf("%s, %d: error: co is ended\n", __FILE__, __LINE__);
		return;
	}

	mco_gtls_co = resume_co;
	mco_ctx_switch(resume_co->switch_back_reg, resume_co->reg);
	mco_gtls_co = NULL;

	return;
}

/* __attribute__((__no_sanitize_address__)) */
void
mco_yield(void)	{
	if (mco_gtls_co)	{
		mco_ctx_switch(mco_gtls_co->reg, mco_gtls_co->switch_back_reg);
	}
	else	{
		printf("%s, %d: error: supposed to be called inside co\n", __FILE__, __LINE__);
	}
}

void *
mco_get_arg(void)	{
	if (mco_gtls_co)	{
		return mco_gtls_co->arg;
	}
	else	{
		return NULL;
	}
}

void *
mco_get_co(void)	{
	return mco_gtls_co;
}

int
mco_is_end(void * co)	{
	mco_t * p;

	p = co;

	return p->is_end;
}

void
mco_destroy(void * co)	{
	mco_t * p;
	p = co;
#ifdef MCO_USE_VALGRIND
	VALGRIND_STACK_DEREGISTER(p->valgrind_stk_id);
#endif
	free(p);
}

/* eof */
